#include <stdio.h>
#include "ros/ros.h"
#include <WarbotsMsg/velocityMsg.h>
#include "std_msgs/Float32.h"
#include "common.h"

#if DEBUG_MODE == 0
#include <roboard.h>  // include this to use the RoBoIO library
//#include <rcservo.h>

#define MOTOR_A_OUT RCSERVO_PINS3
#define MOTOR_A_DIR RCSERVO_PINS4
#define MOTOR_B_OUT RCSERVO_PINS5
#define MOTOR_B_DIR RCSERVO_PINS6
#define MOTOR_C_OUT RCSERVO_PINS7
#define MOTOR_C_DIR RCSERVO_PINS8
#define MOTOR_D_OUT RCSERVO_PINS9
#define MOTOR_D_DIR RCSERVO_PINS10
#endif	

#define CLAMP(x, UB, LB) ( (x > UB) ? (UB) : ((x < LB) ? (LB) : (x)) )

//controller constants
const double kp = 1;
const double ki = 1;

WarbotsMsg::velocityMsg vMsg;

struct mParam_t
{
   bool on;
   double motor_out;
   bool motor_dir;

   double rps_actual;
   double rps_ref;
   double rps_error;
   double rps_out;
   double rps_error_sum;
   double i_term;
   double p_term;
}mParam[4];

void init()
{
   for(int i=0; i<4; i++)
   {
      mParam[i].on = true;
      mParam[i].motor_out = 0;
      mParam[i].motor_dir = 0;
      mParam[i].rps_actual = 0;
      mParam[i].rps_ref = 20;
      mParam[i].rps_error = 0;
      mParam[i].rps_out = 0;
      mParam[i].rps_error_sum = 0;
      mParam[i].i_term = 0;
      mParam[i].p_term = 0;
   }
}

void encoderCallback(const WarbotsMsg::velocityMsg& msg)
{
   mParam[0].rps_actual = msg.v1;
   mParam[1].rps_actual = msg.v2;
   mParam[2].rps_actual = msg.v3;
   mParam[3].rps_actual = msg.v4;
   
   (mParam[0].motor_dir==1)?(mParam[0].motor_dir*(-1)):(mParam[0].motor_dir);
   (mParam[1].motor_dir==1)?(mParam[1].motor_dir*(-1)):(mParam[1].motor_dir);
   (mParam[2].motor_dir==1)?(mParam[2].motor_dir*(-1)):(mParam[2].motor_dir);
   (mParam[3].motor_dir==1)?(mParam[3].motor_dir*(-1)):(mParam[3].motor_dir);
}

void setSpeed(const WarbotsMsg::velocityMsg& msg)
{
   mParam[0].rps_ref = vMsg.v1 = msg.v1;
   mParam[1].rps_ref = vMsg.v2 = msg.v2;
   mParam[2].rps_ref = vMsg.v3 = msg.v3;
   mParam[3].rps_ref = vMsg.v4 = msg.v4;
   //ROS_INFO("robot: speed [%f], [%f], [%f], [%f]\n", vMsg.v1, vMsg.v2, vMsg.v3, vMsg.v4);
}

void PID_control()
{
   for(int i=0; i<4; i++)
   {
      mParam[i].rps_error = mParam[i].rps_ref - mParam[i].rps_actual;
      mParam[i].p_term = kp*mParam[i].rps_error;

      mParam[i].rps_error_sum += mParam[i].rps_error;
      mParam[i].rps_error_sum = CLAMP(mParam[i].rps_error_sum, 100, -100);
      mParam[i].i_term = mParam[i].rps_error_sum*ki;

      mParam[i].motor_out = mParam[i].i_term + mParam[i].p_term;
	
      if (mParam[i].motor_out < 0)
      {
         mParam[i].motor_dir = 1;
         mParam[i].motor_out *= -1;
      }
      else
      {	
         mParam[i].motor_dir = 0;
      }		

      mParam[i].motor_out = CLAMP(mParam[i].motor_out, 80.0, 0.0);
   }
}

void PWM_control()
{
#ifndef MANUAL_CONTROL
   // hard code a value for testing purpose
   for(int i=0; i<4; i++)
   {
      mParam[i].motor_dir = true;
      mParam[i].motor_out = 5.0;
   }
#else
   mParam[0].motor_dir = (vMsg.v1 < 0)?0:1;
   mParam[1].motor_dir = (vMsg.v2 < 0)?0:1;
   mParam[2].motor_dir = (vMsg.v3 < 0)?0:1;
   mParam[3].motor_dir = (vMsg.v4 < 0)?0:1;

   mParam[0].motor_out = fabs(vMsg.v1);
   mParam[1].motor_out = fabs(vMsg.v2);
   mParam[2].motor_out = fabs(vMsg.v3);
   mParam[3].motor_out = fabs(vMsg.v4);

   mParam[0].motor_out = CLAMP(mParam[0].motor_out, 20.0, 0.0);
   mParam[1].motor_out = CLAMP(mParam[1].motor_out, 20.0, 0.0);
   mParam[2].motor_out = CLAMP(mParam[2].motor_out, 20.0, 0.0);
   mParam[3].motor_out = CLAMP(mParam[3].motor_out, 20.0, 0.0);
#endif

#if DEBUG_MODE == 0
   rcservo_OutPin(MOTOR_A_DIR, mParam[0].motor_dir );
   rcservo_SendCPWM(MOTOR_A_OUT, 100L, (unsigned long) mParam[0].motor_out);

   rcservo_OutPin(MOTOR_B_DIR, mParam[1].motor_dir );
   rcservo_SendCPWM(MOTOR_B_OUT, 100L, (unsigned long) mParam[1].motor_out);

   rcservo_OutPin(MOTOR_C_DIR, mParam[2].motor_dir );
   rcservo_SendCPWM(MOTOR_C_OUT, 100L, (unsigned long) mParam[2].motor_out);

   rcservo_OutPin(MOTOR_D_DIR, mParam[3].motor_dir );
   rcservo_SendCPWM(MOTOR_D_OUT, 100L, (unsigned long) mParam[3].motor_out);
#endif

   ROS_INFO("robot: motor 1 [%i], [%f]\n", mParam[0].motor_dir, mParam[0].motor_out);
   ROS_INFO("robot: motor 2 [%i], [%f]\n", mParam[1].motor_dir, mParam[1].motor_out);
   ROS_INFO("robot: motor 3 [%i], [%f]\n", mParam[2].motor_dir, mParam[2].motor_out);
   ROS_INFO("robot: motor 4 [%i], [%f]\n", mParam[3].motor_dir, mParam[3].motor_out);

}

int main(int argc, char **argv) {

   ros::init(argc,argv, "robot");
   ros::NodeHandle n_encoder, nh, n_robot;
   ros::Subscriber encoder_sub = n_encoder.subscribe("encoder_rps", 2, encoderCallback);
   ros::Subscriber Speed_sub = nh.subscribe("SetSpd", 1000, setSpeed);
   ros::Publisher Speed_pub = n_robot.advertise<WarbotsMsg::velocityMsg>("act_mRot_rpm", 1);
   ros::Rate loop_rate(25); 

#if DEBUG_MODE == 0
   roboio_SetRBVer(RB_110);   
   // set PWM/GPIO pins S3 as Servo mode
   if (rcservo_Init(RCSERVO_USEPINS3 + RCSERVO_USEPINS5 + RCSERVO_USEPINS7 + RCSERVO_USEPINS9) == false)
   {
      printf("ERROR: fail to init RC Servo lib (%s)!\n", roboio_GetErrMsg());
      return -1;
   }
   rcservo_EnterPWMMode();  // make all servo-mode pins go into PWM mode

#endif

   init();
    
   while (ros::ok())	
   {
      PID_control();
		
      PWM_control();	


      //while (!rcservo_IsPWMCompleted(MOTOR_OUT));
    	
      //ROS_INFO("Robot: [%f]", motor_out);

      Speed_pub.publish(vMsg);
      ros::spinOnce(); 

      loop_rate.sleep();
    }

#if DEBUG_MODE == 0
    rcservo_StopPWM(MOTOR_A_OUT);
    rcservo_StopPWM(MOTOR_B_OUT);
    rcservo_StopPWM(MOTOR_C_OUT);
    rcservo_StopPWM(MOTOR_D_OUT);
    rcservo_Close();  // close RC Servo lib
#endif	
    
    return(0);
}
